#include <stdint.h>                                                             
#include <stdio.h>                                                              
#include <stdlib.h>   
#include <string.h>
#include <fcntl.h>      
#include <unistd.h>
#include <getopt.h>
#include <sys/ioctl.h>                                                          
#include <linux/spi/spidev.h>   
#include <linux/types.h>
#include <sys/time.h> 

#include "imu-42686.h"


//static const char *device = "/dev/spidev0.0";
static const char *device = "/dev/spidev1.1";
static int s_fd = -1; 
static uint8_t mode = SPI_MODE_0;
static uint8_t bits = 8;  
static uint32_t speed = 24000000;  

int g_gyro_icm42686_accel_fs_sel = ACCEL_FS_4G;
int g_gyro_icm42686_gyro_fs_sel = GYRO_FS_1000;

static float imu_data_to_float( const unsigned char data_h, const unsigned char data_l);

static void pabort(const char *s)
{
	perror(s);
	abort();
}

void spi_init_test()
{
    int ret = 0;         
  
    s_fd = open(device, O_RDWR);  
    if (s_fd < 0)  
        pabort("can't open device");  

    printf("open device sucucess %s\n", device);  
   /* 
    * spi mode 
     */  
    ret = ioctl(s_fd, SPI_IOC_WR_MODE, &mode);  
    if (ret == -1)  
        pabort("can't set spi mode");  
  
    ret = ioctl(s_fd, SPI_IOC_RD_MODE, &mode);  
    if (ret == -1)  
        pabort("can't get spi mode");  
  
    /* 
     * bits per word 
     */  
    ret = ioctl(s_fd, SPI_IOC_WR_BITS_PER_WORD, &bits);  
    if (ret == -1)  
        pabort("can't set bits per word");  
  
    ret = ioctl(s_fd, SPI_IOC_RD_BITS_PER_WORD, &bits);  
    if (ret == -1)  
        pabort("can't get bits per word");  
  
    /* 
     * max speed hz 
     */  
    ret = ioctl(s_fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);  
    if (ret == -1)  
        pabort("can't set max speed hz");  
  
    ret = ioctl(s_fd, SPI_IOC_RD_MAX_SPEED_HZ, &speed);  
    if (ret == -1)  
        pabort("can't get max speed hz");  
 
    printf("spi mode: %d\n", mode);  
    printf("bits per word: %d\n", bits);  
    printf("max speed: %d Hz (%d KHz)\n", speed, speed/1000);  
}

int SPI_Transfer(const uint8_t * tx_buf, uint8_t * rx_buf, int len)
{
	int ret;
	int i;

	struct spi_ioc_transfer tr = {
		.tx_buf = (unsigned long) tx_buf,
		.rx_buf = (unsigned long) rx_buf,
		.len =len,
		.delay_usecs = 0,
	};

	ret = ioctl(s_fd, SPI_IOC_MESSAGE(1), &tr);
	if (ret < 1)
		printf("can't send spi message");
	else {	
		/*printf("SPI Send [Len:%d]\n", len);
		printf("SPI Receive [len:%d]:", len);*/

		for (i = 0; i < len; i++) {
			printf("0x%02x ", rx_buf[i]);	// rx_buf[0]为0x00, rx_buf[1]是有效数据，还需找找原因。
		}
		printf("\n");
	}

	return ret;
}

int8_t user_spi_read(uint8_t reg_addr, uint8_t *data, uint16_t len)
{
	int retval = 0;
	int i = 0, index = 0;
	int tmp = 0;
	int fd = -1;
	char file_name[0x20];
	unsigned char buf[0x20] = {0};
	struct spi_ioc_transfer mesg[1];
	unsigned int spi_num = 1, csn = 1, cur_reg_addr; 
	unsigned int num_reg = 1, reg_width = 1, data_width = 1, reg_order = 1, data_order = 1; //dev_width = 1, 
    uint8_t tempValue[6];

	num_reg = len;
	sprintf(file_name, "/dev/spidev%u.%u", spi_num, csn);

	fd = open(file_name, 0);
	if (fd < 0)
	{
		printf("Open %s error!\n", file_name);
		retval = -1;
		goto end0;
	}

	printf("%s open!\n", file_name);

	tmp = SPI_MODE_0;
	retval = ioctl(fd, SPI_IOC_WR_MODE, &tmp);
	if (retval)
	{
		printf("set spi mode fail!\n");
		retval = -1;
		goto end1;
	}

	memset(mesg, 0, sizeof mesg);

	mesg[0].tx_buf = (__u32)buf;
	mesg[0].rx_buf = (__u32)buf;
	mesg[0].len = reg_width + data_width; 
	mesg[0].speed_hz = 24000000;  
	mesg[0].bits_per_word = 8;
	mesg[0].cs_change = 1;

	memset(buf, 0, sizeof buf);
	memset(tempValue, 0, sizeof tempValue); 

	for (cur_reg_addr = reg_addr, i = 0; cur_reg_addr < reg_addr + num_reg; cur_reg_addr++, i++)
	{
		index = 0;         

		if (reg_width == 1)
		{
			*(__u8 *)(&buf[index]) = (cur_reg_addr | D_SPI_READ_OP) & 0xff;
			index++;
		}
		else
		{
			if (reg_order)
				*(__u16 *)(&buf[index]) = cur_reg_addr & 0xffff;
			else
			{
				*(__u8 *)(&buf[index]) = (cur_reg_addr >> 8) & 0xff;
				*(__u8 *)(&buf[index + 1]) = cur_reg_addr & 0xff;
			}
			index += 2;
		}

		if (data_width == 1)
		{
			*(__u8 *)(&buf[index]) = 0x00;
		}
		else
		{
			*(__u16 *)(&buf[index]) = 0x0000;
		}

		retval = ioctl(fd, SPI_IOC_MESSAGE(1), mesg);
		if (retval != (int)mesg[0].len)
		{
			printf("SPI_IOC_MESSAGE error \n");
			retval = -1;
			goto end1;
		}

		retval = 0;
		if (data_width == 1)
		{
			tmp = *(__u8 *)(&buf[index]);
	
			tempValue[i]=tmp;			
			printf("index is %.x, tmp is %.x, i is %.x, data is %.x \n\n",index, tmp, i, *data); 
		}         
		else
		{
			if (data_order)
				tmp = *(__u16 *)(&buf[index]);
			else
			{
				tmp = *(__u8 *)(&buf[index]) << 8;
				tmp += (*(__u8 *)(&buf[index + 1]));
			}
		}
         
		 *data=tempValue[0]; 
	}

   //copy multi bytes to DATA array!!
	if (i>2){	   
		 for(int j=0;j<6;j++){			 
			*(data+j) = tempValue[j];
			printf("Test :data[%d] is  %d  ,tempValue[%d] is  %d \n", j,*(data+j) ,j,tempValue[j]);  //*data++ data[j]
		 }			
	 }
    
end1:
	close(fd);

end0:
	return retval;
}

int8_t user_spi_write(uint8_t reg_addr, uint8_t data)
{

	int retval = 0;
	int  index = 0;
	int tmp = 0;
	int fd = -1;
	char file_name[0x20];
	unsigned char buf[0x10] = {0};
	struct spi_ioc_transfer mesg[1];
	// unsigned int spi_num = 0, csn = 0; 
    unsigned int spi_num = 1, csn = 1; 
	unsigned int  reg_width = 1, data_width = 1, reg_order = 1, data_order = 1;  //dev_width = 1,

	sprintf(file_name, "/dev/spidev%u.%u", spi_num, csn);

	fd = open(file_name, 0);
	if (fd < 0)
	{
		printf("Open %s error!\n", file_name);
		retval = -1;
		goto end0;
	}

	tmp = SPI_MODE_0;
	retval = ioctl(fd, SPI_IOC_WR_MODE, &tmp);
	if (retval)
	{
		printf("set spi mode fail!\n");
		retval = -1;
		goto end1;
	}

	memset(mesg, 0, sizeof mesg);

	mesg[0].tx_buf = (__u32)buf;
	mesg[0].rx_buf = (__u32)buf;
	mesg[0].len = reg_width + data_width; //dev_width +
	mesg[0].speed_hz = 24000000;
	mesg[0].bits_per_word = 8;
	mesg[0].cs_change = 1;
   usleep(2000); //add test 
	memset(buf, 0, sizeof buf);

	if (reg_width == 1)
	{
		*(__u8 *)(&buf[index]) = reg_addr & 0xff;
		index++;
	}
	else
	{
		if (reg_order)
			*(__u16 *)(&buf[index]) = reg_addr & 0xffff;
		else
		{
			*(__u8 *)(&buf[index]) = (reg_addr >> 8) & 0xff;
			*(__u8 *)(&buf[index + 1]) = reg_addr & 0xff;
		}
		index += 2;
	}

	if (data_width == 1)
		*(uint8_t *)(&buf[index]) = (data) & 0xff;
	else
	{
		if (data_order)
			*(__u16 *)(&buf[index]) = (data) & 0xffff;

		else
		{
			*(__u8 *)(&buf[index]) = (data >> 8) & 0xff;
			*(__u8 *)(&buf[index + 1]) = (data) & 0xff;
		}
	}

	// reverse8(buf, mesg[0].len);

	retval = ioctl(fd, SPI_IOC_MESSAGE(1), mesg);
	if (retval != (int)mesg[0].len)
	{
		printf("SPI_IOC_MESSAGE error \n");
		retval = -1;
		goto end1;
	}
	retval = 0;

end1:
	close(fd);

end0:
	return retval;
}

int8_t user_spi_readACC(uint8_t reg_addr, uint8_t *data, uint16_t len)
{
	int fd;
	int ret = -1;
	int tmp = 0;
	unsigned char buf[0x10] = {0};
	struct spi_ioc_transfer tr[1];
   
	fd = open("/dev/spidev0.0", O_RDWR);
	if (fd < 0)
	{
		printf("failed to open spidev0.0\n");
	}

	tmp = SPI_MODE_0; //  | SPI_LSB_FIRST;

	ret = ioctl(fd, SPI_IOC_WR_MODE, &tmp);
	if (ret)
	{
		printf("set spi mode fail!\n");
		ret = -1;
		goto end_spi_read;
	}
	
	memset(tr, 0, sizeof tr);
	tr[0].tx_buf = (__u32)buf;
	tr[0].rx_buf = (__u32)buf;
	tr[0].len = 2; 
	tr[0].speed_hz = 24000000;
	tr[0].bits_per_word = 8;
	tr[0].cs_change = 1;
    
	buf[0] = reg_addr;
	for (size_t i = 0; i < len; i++)
	{
		buf[0] = (reg_addr + i) | D_SPI_READ_OP;
		ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr);
		*(data+i) = buf[1];
		printf("Test Data %d ,buf[1] is 0x%x \n",*(data+i),buf[1]);
		//usleep(1000);
	}

    if (ret != tr[0].len) {
        printf("SPI_IOC_MESSAGE error \n");
        ret = -1;
        goto end_spi_read;
    }
   ret=0;

end_spi_read:
	close(fd);

	return ret;
}

void spi_test()
{
	uint8_t r_buf[16] = {0};
	//uint8_t w_buf[12] = {0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2a};
	uint8_t w_buf[1] = {0x75 | D_SPI_READ_OP};
	uint8_t ag_buf[1] = {0x1f | D_SPI_READ_OP};
	uint8_t id = 0x0;
	int s = 0;

	spi_init_test();

	while (1) {
		SPI_Transfer(w_buf, r_buf, 2);
		SPI_Transfer(ag_buf, r_buf, 13);
		//user_spi_read(0x75, &id, 1);		
		//user_spi_read(0x1f, r_buf, 12);
		user_spi_readACC(0x75, &id, 1);	
		printf("id = %#x\n", id);
		printf("%d\n", s++);
		usleep(5*1000);

	}
}

static unsigned long long GetSystemClock()
{
    struct timeval tv;
	unsigned long long tv_sen = 0, gltime = 0;

    gettimeofday(&tv, NULL);

    tv_sen = tv.tv_sec;
    gltime = tv_sen * 1000 + tv.tv_usec / 1000;

    return gltime;
}

void imu42686_init()
{
    uint8_t ret = 0;

    user_spi_write(REG_BANK_SEL, 0x00);
    usleep(50);

    user_spi_write(DEVICE_CONFIG, 0x00);
    usleep(50);

    if (user_spi_read(WHO_AM_I, &ret, 1) >= 0)
        printf("--- icm42686 ID = %#x ---\r\n", ret);

    if (user_spi_read(WHO_AM_I, &ret, 1) >= 0)
        printf("--- icm42686 ID = %#x ---\r\n", ret);
	/*
	* GYRO_CONFIG0:Gyro Full scale and sampling rate config
	* GYRO_FS_SEL[bit5-7],GYRO_ODR[bit0-3],page 58 of 105
	* GYRO_FS_SEL:
		0x00:+-4000dps(default); 0x02:+-2000dps; 0x40:+-1000dps;0x60:+-500dps 
	* GYRO_ODR:
		0x00:Reserved; 0x01:32KHz; 0x02:16KHz; 0x03:8KHz; 0x04:4KHz; 
		0x05:2KHz; 0x06:1KHz(default); 0x07:200Hz; 0x08:100Hz; 
	*/
    user_spi_write(GYRO_CONFIG0,  0x46);	//+-1000dps//1KHz

	/*
	* GYRO_CONFIG1: address 0x51; default:0x16
	* TEMP_FILT_BW:[bit5-7],GYRO_UI_FILT_ORD:[bit2-3],GYRO_DEC2_M2_ORD:[bit0-1]
	* default value: DLPF BW=4000Hz,DLPF Latency=0.125ms
	*	UI filt ord:2th order; gyro_dec2_m2_ord:3th order
	*/
    user_spi_write(GYRO_CONFIG1,  0x16);

	/*
	* ACCEL_CONFIG0
	* ACCEL_FS_SEL[bit5-7] and ACCEL_ODR[bit0-3]) address=0x50,value default 0x06
	* ACCEL_FS_SEL: 000: +-32g; 001:+-16g; 010:+-8g; 011:+-4g; 100:+-2g; 
	* ACCEL_ODR:0001:32KHz; 0010:16KHz; 0011: 8KHz; 0100: 4KHz;0101: 2KHz;0110: 1KHz;
	*	0111: 200Hz; 1000: 1000Hz; 1001:50Hz; 1010:25Hz 
	*/
    user_spi_write(ACCEL_CONFIG0,  0x86);/* full scale: +-2g,accel sampling rate: 1KHz*/

    /*
	* ACCEL_CONFIG1:order for ACCEL_UI_FILT_ORD[bit3-4] and ACCEL_DEC2_M2_ORD [bit1-2]
	* Address:0x53,default value:0x0D
	* ACCEL_UI_FILT_ORD: 00:1th ; 01:2th; 10:3th; 11:reserved 
	* ACCEL_DEC2_M2_ORD: 00:1th ; 01:2th; 10:3th; 11:reserved 
	*/
	user_spi_write(ACCEL_CONFIG1,  0x0D);

	/*
	* GYRO_ACCEL_CONFIG0: bandwidth for accel LPF and Gyro LPF; 
	* Address=0x52,default value:0x11; 
	* ACCEL_UI_FILT_BW:[bit4-7],default:1,BW=max(400Hz,ODR)/4
	* GYRO_UI_FILT_BW:[bit0-3],default:1,BW=max(400Hz,ODR)/4
	*/
	user_spi_write(GYRO_ACCEL_CONFIG0, 0x00);//1st order	P76; R-> 0x52=0x00

	/* 
	* INT1:active low,open drain,pulsed mode
	* INT2:active low,open drain,pulsed mode
	*/
	user_spi_write(INT_CONFIG, 0x00);

	/* stream-to-fifo mode */
	user_spi_write(FIFO_CONFIG, 0x01<<6);
	
	/* Disables de-assert duration, Otherwise, output 4K frequency */
	user_spi_write(INT_CONFIG1, 0x60);	

	/* TMST_TO_REGS_EN:1,TMST_FSYNC_EN:1,TMST_EN:1 */
	user_spi_write(TMST_CONFIG, 0x13);

	/* FIFO count in records pakage of data,
	FIFO count and data are reported in big endian format  
	*/
	user_spi_write(INTF_CONFIG0, 0x70);

	/* set UI data ready interrupt touted to INT1 */
	user_spi_write(FIFO_SOURCE0, 0x08);

	/*
	* FIFO_CONFIG1: Address=0x5F,default value:0x00; 
	* bit0:FIFO_ACCEL_EN; 
	* bit1:FIFO_GYRO_EN; 
	* bit2:FIFO_TEMP_EN; 
	*/
	user_spi_write(FIFO_CONFIG1, FIFO_CONFIG1_DATA);//GYRO and ACCEL FIFO enable	R-> 0x5f=0x03

	/*
	* PWR_MGMT0: Address=0x4E,default value:0x00; 
	* ACCEL_MODE[bit0-1]:11:Places accelerometer in Low Noise (LN) Mode
	* GYRO_MODE[bit2-3]:11:Places gyroscope in Low Noise (LN) Mode
	*/
	user_spi_write(PWR_MGMT0, PWR_MGMT0_DATA);	// 1st order	P72	R->0x4e=0x0f

}

float Gyro_ICM42686_AccelData2Gravity( const unsigned char data_h, const unsigned char data_l )
{
	float f_val;
	float accel_1g_count;

	switch( g_gyro_icm42686_accel_fs_sel ) {
		case ACCEL_FS_2G:	// +-2g Full scale.
			accel_1g_count = 4096.0;	// typ.
			break;
		case ACCEL_FS_4G:	// +-4g Full scale.
			accel_1g_count = 8192.0;	// typ.
			break;
		case ACCEL_FS_8G:	// +-8g Full scale.
			accel_1g_count = 4096.0;	// typ.
			break;
		case ACCEL_FS_16G:	// +-16g Full scale.
			accel_1g_count = 2048.0;	// typ.
			break;
		default:
			printf( "%s(): unknown ACCEL_FS_SEL. 0x%x\n", __func__, g_gyro_icm42686_accel_fs_sel );	/* pgr0822 */
			accel_1g_count = 4096.0;	// fail safe
	}

	f_val = imu_data_to_float( data_h, data_l );
	
	return (f_val / accel_1g_count);
}

float Gyro_ICM42686_GyroData2Gravity( const unsigned char data_h, const unsigned char data_l )
{
	float f_val;
	float gyro_1degree_count;

	switch( g_gyro_icm42686_gyro_fs_sel ) {
		case GYRO_FS_250:	// +-250 degree/sec Full scale.
			gyro_1degree_count = 131.0;	// typ.
			break;
		case GYRO_FS_500:	// +-500 degree/sec Full scale.
			gyro_1degree_count = 65.5;	// typ.
			break;
		case GYRO_FS_1000:	// +-1000 degree/sec Full scale.
			gyro_1degree_count = 32.8;	// typ.
			break;
		case GYRO_FS_2000:	// +-2000 degree/sec Full scale.
			gyro_1degree_count = 16.4;	// typ.
			break;
		default:
			printf( "%s(): unknown GYRO_FS_SEL. 0x%x\n", __func__, g_gyro_icm42686_gyro_fs_sel );	/* pgr0822 */
			gyro_1degree_count = 131.0;	// fail safe
	}

	f_val = imu_data_to_float( data_h, data_l );

	return (f_val / gyro_1degree_count);	
}

/*
 * radian convertion angle.
 */
float radian_to_angle(float *ft)
{
	return *ft * (180/M_PI);
}

/*
 * angle convertion radian.
 */
float angle_to_radian(float *ft)
{
	return *ft * (M_PI/180);
}

int imu_original_value(unsigned char * data, struct icm42686_data * imu)
{
    #define _R_OFFSET 1
    imu->temperature.deg = (data[0+_R_OFFSET] & 0xff) << 8 | (data[1+_R_OFFSET] & 0xff);
	imu->acc.x = (data[2+_R_OFFSET] & 0xff) << 8 | (data[3+_R_OFFSET] & 0xff);
	imu->acc.y = (data[4+_R_OFFSET] & 0xff) << 8 | (data[5+_R_OFFSET] & 0xff);
	imu->acc.z = (data[6+_R_OFFSET] & 0xff) << 8  | (data[7+_R_OFFSET] & 0xff);
	imu->gyro.x = (data[8+_R_OFFSET] & 0xff) << 8  | (data[9+_R_OFFSET] & 0xff);
	imu->gyro.y = (data[10+_R_OFFSET] & 0xff) << 8 | (data[11+_R_OFFSET] & 0xff);
	imu->gyro.z = (data[12+_R_OFFSET] & 0xff) << 8 | (data[13+_R_OFFSET] & 0xff);

    printf("0x%02x, 0x%04x, 0x%04x, 0x%04x, 0x%04x, 0x%04x, 0x%04x\n", 
                            imu->temperature.deg,
                            imu->acc.x, imu->acc.y, imu->acc.z,
                            imu->gyro.x, imu->gyro.y, imu->gyro.z);

	return 0;
}

static float imu_data_to_float( const unsigned char data_h, const unsigned char data_l)
{
	union 
	{
		short data_short;
		unsigned short data_ushort;
	} us2s;

	us2s.data_ushort = (((unsigned short)data_h) << 8) | data_l;

	return (float)us2s.data_short;
}

void imu_data_resolving(unsigned char * pimu, struct temperature * tmp, struct imu_acc * acc, struct imu_gry * gry)
{
    tmp->deg = ((pimu[0+1] & 0xff) << 8 | (pimu[1+1])) / 132.48 + 25;

	acc->ax = (imu_data_to_float(pimu[2+1], pimu[3+1])) * 4 / 65536.0;
	acc->ay = (imu_data_to_float(pimu[4+1], pimu[5+1])) * 4 / 65536.0;
	acc->az = (imu_data_to_float(pimu[6+1], pimu[7+1])) * 4 / 65536.0;

	gry->gx = (imu_data_to_float(pimu[8+1], pimu[9+1])) * 2000 / 65536.0;
	gry->gy = (imu_data_to_float(pimu[10+1], pimu[11+1])) * 2000 / 65536.0;
	gry->gz = (imu_data_to_float(pimu[12+1], pimu[13+1])) * 2000 / 65536.0;

    /*printf("---temperature--- deg=%f\n", tmp->deg);
	printf("---acc--- x=%f y=%f z=%f\n", acc->ax, acc->ay, acc->az);
    //printf("---gry--- x=%f y=%f z=%f\n", gry->gx, gry->gy, gry->gz);
	printf("---gry--- x=%f y=%f z=%f\n", angle_to_radian(&gry->gx), 
                                        angle_to_radian(&gry->gy), angle_to_radian(&gry->gz));*/
}

#define HEX_BUF_SIZE    41

int main()
{
	uint8_t r_buf[16] = {0};
	uint8_t ag_buf[1] = {0x1d | D_SPI_READ_OP};
	int s = 0;
    struct imu_acc acc = {.ax = 0, .ay = 0, .az = 0};
	struct imu_gry gry = {.gx = 0, .gy = 0, .gz = 0};
    struct temperature tmp = {.deg = 0.0};
    FILE *imu_file = fopen("imu-data.txt", "w+");
    FILE *hex_file = fopen("hex-data.txt", "w+");
    char imu_buf[96] = {0};
    char hex_buf[HEX_BUF_SIZE] = {0};
    int ret = -1;
    unsigned long long system_time = 0;
    struct icm42686_data imu_original = {0};

    imu42686_init();

	spi_init_test();

	while (1) {
		SPI_Transfer(ag_buf, r_buf, 15);	
        
        #if 0
        imu_original_value(r_buf, &imu_original);
        #endif
        
        #if 1
        imu_data_resolving(r_buf, &tmp, &acc, &gry);

        if (imu_file == NULL) {
            printf("Open file imu-data.txt error!");
            return -1;
        } else {
            system_time = GetSystemClock();
            memset(imu_buf, 0, 96);
            sprintf(imu_buf, "%lld, %lf, %lf,%lf,%lf,%lf,%lf,%lf\n", \
                        system_time, tmp.deg, angle_to_radian(&gry.gx), angle_to_radian(&gry.gy), 
                        angle_to_radian(&gry.gz), acc.ax, acc.ay, acc.az);
            
            ret = fwrite(imu_buf, 1, sizeof(imu_buf), imu_file);
            if (ret != sizeof(imu_buf)) {
                printf("Failed to write [%s] by [%d]!", "imu-data.txt", ret);
            }

            memset(hex_buf, 0, HEX_BUF_SIZE);
            sprintf(hex_buf, "%04x, %04x, %04x, %04x, %04x, %04x, %04x\n", 
                        r_buf[1]<<8 | r_buf[2], r_buf[3]<<8 | r_buf[4],
                        r_buf[5]<<8 | r_buf[6], r_buf[7]<<8 | r_buf[8],
                        r_buf[9]<<8 | r_buf[10], r_buf[11]<<8 | r_buf[12],
                        r_buf[13]<<8 | r_buf[14]);
            
            ret = fwrite(hex_buf, 1, sizeof(hex_buf), hex_file);
            if (ret != sizeof(hex_buf)) {
                printf("Failed to write [%s] by [%d]!", "hex-data.txt", ret);
            }

            //printf("%d\n", s++);
        }
        #endif

		usleep(5*1000);
	}

    fclose(imu_file);

	return 0;
}

